//----------------------------------------------------------------------------
// Filename                : current.c
//
// Functional Description  : Current measurement support functions
//----------------------------------------------------------------------------

#include "DRV922xx_trapezoidal_sensored.h"
#include "message_protocol_application.h"
void sample_peak_current_setup(void)
{
	// ------------------------------------
	// Sample peak current
	// ------------------------------------
	drv922xxAdcStartAndDelay(ADC_CHANNEL_CS);
}
/*  \fn void sample_peak_current(void)
 *  \brief Samples the peak current and performs firmware current limit function
 */
void sample_peak_current(void)
{
	static unsigned int filter [16] = {0xFF, 0xFF, 0xF0, 0xE7, 0xe9, 0xeA, 0xe5, 0xee, 0xe5, 0xef, 0xf1, 0xf4, 0xf3, 0xf2, 0xfa, 0x1};
	static unsigned int index_current = 0;
	static unsigned int index_oldest = 0;
	static unsigned int sum = 0;
	static unsigned int init = 1;
	static unsigned int size = 16;
	#define SHIFT_BITS 6
	
//	int table_index = (saved_com_state - 1) << 3;			// Point to current index in table
	
	drv922xxAdcStartAndDelay(ADC_CHANNEL_CS);
//	sample_peak_current_setup();
	// ------------------------------------
	// Check if we missed the PWM edge - Only calculate the max current if we did not
	// ------------------------------------
//	if ((P2IN & Comm_Values[table_index+1]) != 0)	// Look at P2IN & P2SEL to see if correct HS gate is high
//	{
		// ------------------------------------
		// Calculate Max current from ADC read and store in buffer
		// ------------------------------------
		MaxiCurrent_high = drv922xxSpiRead(SPI_ADC1_READ);
		MaxiCurrent_low = drv922xxSpiRead(SPI_ADC2_READ);
		AppCurrent = ((MaxiCurrent_high & 0x3) << 8) | MaxiCurrent_low;      
		
		if (init == 1)
		{
//			sum = sum + filter[index_current++];//AppCurrent;
			sum = sum + AppCurrent;
			filter[index_current++] = AppCurrent;
			if (index_current == 15)
			{
				init = 0;
			}
			AppCurrentFiltered = AppCurrent;
		}
		else
		{
			sum = sum - filter[index_oldest] + AppCurrent;
			AppCurrentFiltered = (4*sum)>>(SHIFT_BITS);
			filter[index_current++] = AppCurrent;
			if (index_current == 16)
			{
				index_current = 0;
				index_oldest = 1;
			}
			else
			{
				if (index_current == 15)
				{
					index_oldest = 0;
				}
				else
				{
					index_oldest = index_current+1;
				}
			}	
		}
//		AppCurrentFiltered = (3*(AppCurrentFiltered>>2)) + (AppCurrent>>2);
//	}
//	
//	AppOverCurrent = 0;
	if (AppCurrent > AppIfilterMax)															// If we are over the high current threshold, decrement the duty cycle
	{
		AppOverCurrent = 50;
//		send_cmd(CmdCurrent, AppCurrent);
		if (AppThrottleVoltageFiltered < 1)
			AppThrottleVoltageFiltered = 0;
		else
			AppThrottleVoltageFiltered -= 1;
			
		// ------------------------------------
	    // Calculate new PWM duty cycle
	    // ------------------------------------
		pwm_duty = throttle_voltage_to_pwm(AppThrottleVoltageFiltered);
		
		// ------------------------------------
	    // Set new duty cycle
	    // ------------------------------------	
	    set_duty();
	}
	else if (AppCurrent > AppIfilterMid)
	{
		AppOverCurrent = 25;
	}
	else {
		AppOverCurrent = 0;
	}
}
